
#include "MotionFrame.h"

#include "../../XMLTools.h"
#include "../../MathTools.h"

namespace MMM
{

    MotionFrame::MotionFrame(unsigned int ndof)
    {
        this->ndof = ndof;
        timestep = 0.0f;
        joint = Eigen::VectorXf::Zero(ndof);
        joint_vel = Eigen::VectorXf::Zero(ndof);
        joint_acc = Eigen::VectorXf::Zero(ndof);
    }

    MotionFrame::MotionFrame(const MotionFrame &inpSrc)
    {
        this->ndof = inpSrc.ndof;
        timestep = inpSrc.timestep;
        joint = inpSrc.joint;
        joint_vel = inpSrc.joint_vel;
        joint_acc = inpSrc.joint_acc;
        motionFrameEntries = inpSrc.motionFrameEntries;
    }

    std::string MotionFrame::toXML()
    {
        std::string tab1 = "\t\t\t";
        std::string tab2 = "\t\t\t\t";
        std::stringstream res;

        res << tab1 << "<MotionFrame>" << endl;
        res << tab2 << "<Timestep>" << timestep << "</Timestep>" << endl;
        std::map<std::string, MotionFrameEntryPtr>::iterator i = motionFrameEntries.begin();
        while (i != motionFrameEntries.end())
        {
            res << i->second->toXML();
            i++;
        }
        if(joint.rows()>0)
        {res << tab2 << "<JointPosition>";
            for (int j=0;j<(int)joint.rows();j++)
            {
                res << joint[j] << " ";
            }
            res << "</JointPosition>" << endl;
        }
        if(joint_vel.rows() > 0)
        {
            res << tab2 << "<JointVelocity>";
            for (int j=0;j<(int)joint_vel.rows();j++)
            {
                res << joint_vel[j] << " ";
            }
            res << "</JointVelocity>" << endl;
        }
        if(joint_acc.rows() > 0)
        {
            res << tab2<< "<JointAcceleration>";
            for (int j=0;j<(int)joint_acc.rows();j++)
            {
                res << joint_acc[j] << " ";
            }
            res << "</JointAcceleration>" << endl;
        }
        res << tab1 << "</MotionFrame>" << endl;
        return res.str();
    }

    bool MotionFrame::addEntry( const std::string &name, MotionFrameEntryPtr entry )
    {
        std::string lc = name;
        XML::toLowerCase(lc);
        motionFrameEntries[lc] = entry;
        return true;
    }

    MMM::MotionFrameEntryPtr MotionFrame::getEntry( const std::string &name )
    {
        std::string lc = name;
        XML::toLowerCase(lc);
        if (!hasEntry(lc))
        {
            MMM_INFO << "Could not find entry with name:" << name << endl;
            return MMM::MotionFrameEntryPtr();
        }
        return motionFrameEntries[lc];
    }

    bool MotionFrame::hasEntry( const std::string &name ) const
    {
        std::string lc = name;
        XML::toLowerCase(lc);
        if (motionFrameEntries.find(lc) != motionFrameEntries.end())
        {
            return true;
        }
        return false;
    }

    bool MotionFrame::removeEntry( const std::string &name )
    {
        std::string lc = name;
        XML::toLowerCase(lc);
        if (hasEntry(lc))
            motionFrameEntries.erase(lc);
        return true;
    }

    Eigen::Vector3f MotionFrame::getRootPos()
    {
        if (!hasEntry("RootPosition"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootPosition");
        RootPositionPtr r = boost::dynamic_pointer_cast<RootPosition>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_pos;
    }

    Eigen::Vector3f MotionFrame::getRootPosVel()
    {
        if (!hasEntry("RootPositionVelocity"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootPositionVelocity");
        RootPositionVelPtr r = boost::dynamic_pointer_cast<RootPositionVel>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_pos_vel;
    }

    Eigen::Vector3f MotionFrame::getRootPosAcc()
    {
        if (!hasEntry("RootPositionAcceleration"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootPositionAcceleration");
        RootPositionAccPtr r = boost::dynamic_pointer_cast<RootPositionAcc>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_pos_acc;
    }


    Eigen::Vector3f MotionFrame::getRootRot()
    {
        if (!hasEntry("RootRotation"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootRotation");
        RootOrientationPtr r = boost::dynamic_pointer_cast<RootOrientation>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_ori;
    }

    Eigen::Vector3f MotionFrame::getRootRotVel()
    {
        if (!hasEntry("RootRotationVelocity"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootRotationVelocity");
        RootOrientationVelPtr r = boost::dynamic_pointer_cast<RootOrientationVel>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_ori_vel;
    }

    Eigen::Vector3f MotionFrame::getRootRotAcc()
    {
        if (!hasEntry("RootRotationAcceleration"))
            return Eigen::Vector3f::Zero();
        MotionFrameEntryPtr e = getEntry("RootRotationAcceleration");
        RootOrientationAccPtr r = boost::dynamic_pointer_cast<RootOrientationAcc>(e);
        if (!r)
            return Eigen::Vector3f::Zero();
        return r->root_ori_acc;
    }

    Eigen::Matrix4f MotionFrame::getRootPose()
    {
        Eigen::Matrix4f result;
        result = Math::poseRPYToMatrix4f(getRootPos(),getRootRot());
        return result;
    }

    bool MotionFrame::setRootPose(const Eigen::Matrix4f &pose)
    {
        Eigen::VectorXf p = Math::matrix4fToPoseRPY(pose);
        return (setRootPos(p.head(3)) && setRootRot(p.tail(3)));
    }

    bool MotionFrame::setRootPos(const Eigen::Vector3f &pos)
    {
        RootPositionPtr r(new RootPosition());
        r->root_pos = pos;
        return addEntry("RootPosition", r);
    }

    bool MotionFrame::setRootPosVel(const Eigen::Vector3f &posVel)
    {
        RootPositionVelPtr r(new RootPositionVel());
        r->root_pos_vel = posVel;
        return addEntry("RootPositionVelocity", r);
    }

    bool MotionFrame::setRootPosAcc(const Eigen::Vector3f &posAcc)
    {
        RootPositionAccPtr r(new RootPositionAcc());
        r->root_pos_acc = posAcc;
        return addEntry("RootPositionAcceleration", r);
    }

    bool MotionFrame::setRootRot(const Eigen::Vector3f &rot)
    {
        RootOrientationPtr r(new RootOrientation());
        r->root_ori = rot;
        return addEntry("RootRotation", r);
    }

    bool MotionFrame::setRootRotVel(const Eigen::Vector3f &rotVel)
    {
        RootOrientationVelPtr r(new RootOrientationVel());
        r->root_ori_vel = rotVel;
        return addEntry("RootRotationVelocity", r);
    }

    bool MotionFrame::setRootRotAcc(const Eigen::Vector3f &rotAcc)
    {
        RootOrientationAccPtr r(new RootOrientationAcc());
        r->root_ori_acc = rotAcc;
        return addEntry("RootRotationAcceleration", r);
    }


}

